#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     BL,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     FL,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     BR,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     FR,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     Arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Lift,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    WristLift,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

#define SERVO_START_POS 0
#define SERVOS_MAX_VALUE 250
#define SERVOS_MIN_VALUE 0
#define SERVOS_WAIT_TIME_10MSEC 1

int wristPos = 0;

int threshold = 20;

void Forward (int power, int duration)						// Positive power means wheels going forward
{
	motor[FL] = power;
	motor[BL] = power;
	motor[FR] = power;
	motor[BR] = power;
	wait1Msec(duration);
}

void Backward (int power, int duration)
{
	motor[FL] = -1*power;
	motor[BL] = -1*power;
	motor[FR] = -1*power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void turnL (int power, int duration)
{
	motor[FL] = -1*power;
	motor[BL] = -1*power;
	motor[FR] = power;
	motor[BR] = power;
	wait1Msec(duration);
}

void turnR (int power, int duration)
{
	motor[FL] = power;
	motor[BL] = power;
	motor[FR] = -1*power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void initializeRobot ()
{
	motor[FL] = 0;
	motor[BL] = 0;
	motor[FR] = 0;
	motor[BR] = 0;
	wait1Msec(1000);
}

task main ()
{
	initializeRobot();
	while (true)
	{
		getJoystickSettings(joystick);

	if(joystick.joy1_TopHat == 0)
	{
		Forward(40,25); //Forward precision power
	}
	else if(joystick.joy1_TopHat== 2)
	{
		turnR(40,25); //Turn Right precision power
	}
	else if(joystick.joy1_TopHat==4)
	{
		Backward(40,25); //Backward precision power
	}
	else if(joystick.joy1_TopHat==6)
	{
		turnL(40,25); //Turn Left precision power
	}
	else if(joy1Btn(4))
	{
		Forward(80,25); //Forward Fast
	}
	else if(joy1Btn(3))
	{
		turnR(80,25);	//Turn Right Fast
	}
	else if(joy1Btn(2))
	{
		Backward(80,25); //Backward Fast
	}
	else if(joy1Btn(1))
	{
		turnL(80,25); //Turn Right Fast
	}
	else
	{
		motor[FL]=0;
		motor[BL]=0;
		motor[FR]=0;
		motor[BR]=0;
	}
	//This is where ARM PROGRAM Begins

		//Lift

		if(abs(joystick.joy2_y1) > threshold)
		{
			if(joystick.joy2_y1 > 0)
			{
				motor[Lift] = 65;
			}
			else if(joystick.joy2_y1 < 0)
			{
				motor[Lift] = -1;
			}
		}
		else
		{
			motor[Lift] = 0;
		}

		//Arm

		if(abs(joystick.joy2_y2) > threshold)
		{
			if(joystick.joy2_y2 < 0)
			{
				motor[Arm] = 20; //Arm power started at 30 power when there was a 1 to 16 gear reduction, but when we changed it to a 1 to 4 gear reduction ratio,we had to divide the power by 4
			}
			else if(joystick.joy2_y2 > 0)
			{
				motor[Arm] = -25;
			}
		}

		else
		{
			motor[Arm] = 0;
		}

		{
			//WRIST

	if(joy2Btn(5))
		{
			if (wristPos < SERVOS_MAX_VALUE)
			{
				wristPos = wristPos+3;
			}
		}
		else if(joy2Btn(7))
		{
			if (wristPos > SERVOS_MIN_VALUE)
				{
					wristPos = wristPos-3;
				}
		}
			servoTarget[WristLift] = wristPos;
			wait10Msec(1);
		}
	}
}
